#include "OMPLPlanner.h"
#include "AiBox_LOG.h"
#include "MqttPlannerServer.h"
#include "cJSON.h"
using namespace Planner;
ob::RealVectorBounds OMPLPlanner::_ompl_bounds(2);
OMPLPlanner::OMPLPlanner(int vechicle_id,
                         const LocationInfo& start_point,
                         const LocationInfo& target_point,
                         const std::map<int,LocationInfo>& obstacle_map,
                         space_type_t space_type,
                         float safe_distance)
:_this_planner_finished(false)
,_obstacle_map(obstacle_map)
,_safe_distance(safe_distance)
,_vechicle_id(vechicle_id)
{
    addToManage(std::shared_ptr<OMPLPlanner>(this));
    //default bound
    _ompl_bounds.setLow(-100.0);
    _ompl_bounds.setHigh(100.0);
    switch(space_type){
        case SPACE_TYPE_SE2:
            _ompl_space_ptr=std::make_shared<ob::SE2StateSpace>();
            _ompl_space_ptr->as<ob::SE2StateSpace>()->setBounds(_ompl_bounds);
            break;
        default :
            AIBOX_LOGE("Unkown space type.");
            _this_planner_finished=true;
            return ;
    }
    //set the ompl planner setup
    _ompl_setup_ptr=std::make_shared<og::SimpleSetup>(_ompl_space_ptr);
    _ompl_setup_ptr->setStateValidityChecker([this](const ob::State *state){
            return this->isSateValid(state);
        });
    //set target 
    ob::ScopedState<> start(_ompl_space_ptr);
    ob::ScopedState<> goal(_ompl_space_ptr);
    start[0] = start_point.getNEVector()._n;
    start[1] = start_point.getNEVector()._e;
    start[2] = 0.0;
    goal[0] = target_point.getNEVector()._n;
    goal[1] = target_point.getNEVector()._e;
    goal[2] = 0.0;
    _ompl_setup_ptr->setStartAndGoalStates(start, goal);
    //new path plan thread
    _this_planner_finished=false;
    _ompl_planner_thread=std::make_shared<std::thread>(&OMPLPlanner::OMPLPlannerThread,this);
    if(_ompl_planner_thread.get()==nullptr){   
        AIBOX_LOGE("Creat planner thread err.");
        _this_planner_finished=true;
    }
}
void 
OMPLPlanner::OMPLPlannerThread()
{
    ob::PlannerStatus solved = _ompl_setup_ptr->solve(5.0);
    if(solved){
        //solved the path,need creat json and send to client
        _ompl_setup_ptr->simplifySolution();
        MqttPlannerServer::getInstance()->pubPathMsg(_vechicle_id,pathJson( _ompl_setup_ptr->getSolutionPath().getStates()));
    }
    _this_planner_finished=true;
}
bool 
OMPLPlanner::isSateValid(const ob::State *state)
{
    double n=state->as<ob::SE2StateSpace::StateType>()->getX();
    double e=state->as<ob::SE2StateSpace::StateType>()->getY();
    LocationInfo temp(n,e);
    for(const auto& item:_obstacle_map){
        if(item.first==_vechicle_id){
            continue;
        }
        if(item.second.distanceTo(temp)<_safe_distance){
            return false;
        }
    }
    return true;
}
std::string 
OMPLPlanner::pathJson(const std::vector<ob::State *> & path_vector)
{
    cJSON *pJsonRoot =cJSON_CreateObject();
    if (NULL == pJsonRoot){
		//error happend here
		AIBOX_LOGE("create path json root fail");
		return "";
	}
    //{"ID":,"PATH"：{["LAT":xxx,"LON":xxx],["LAT":xxx,"LON":xxx]}}
    cJSON_AddNumberToObject(pJsonRoot,"ID",_vechicle_id);
    cJSON *pJsonPathArry=cJSON_CreateArray();
    if(NULL==pJsonPathArry){
        cJSON_Delete(pJsonRoot);
        return "";
    }
    for(const auto& item: path_vector){
        cJSON *pJsonPath =cJSON_CreateObject();
        if (NULL == pJsonPath){
		    //error happend here
		    AIBOX_LOGE("create path json fail");
		    continue;
	    }
        double n=item->as<ob::SE2StateSpace::StateType>()->getX();
        double e=item->as<ob::SE2StateSpace::StateType>()->getY();
        LocationInfo temp(n,e);
        cJSON_AddNumberToObject(pJsonPath,"LAT",temp.getLat());
        cJSON_AddNumberToObject(pJsonPath,"LON",temp.getLon());
        cJSON_AddItemToArray(pJsonPathArry,pJsonPath);
    }
    cJSON_AddItemToObject(pJsonRoot,"PATH",pJsonPathArry);
    std::string json_string(cJSON_Print(pJsonRoot));
    cJSON_Delete(pJsonRoot);
    return json_string;
}
//commont property

void 
OMPLPlanner::setOMPLBounds(double low_bound,double hight_bound)
{
    _ompl_bounds.setLow(low_bound);
    _ompl_bounds.setHigh(hight_bound);
}
//manage
std::shared_ptr<std::thread> OMPLPlanner::_planner_manage_thread=nullptr;
std::list<std::shared_ptr<OMPLPlanner>> OMPLPlanner::_ompl_planner_list;
void 
OMPLPlanner::addToManage(const std::shared_ptr<OMPLPlanner>& planner)
{
    _ompl_planner_list.push_back(planner);
    if(_planner_manage_thread.get()==nullptr){
        _planner_manage_thread=std::make_shared<std::thread>(&OMPLPlanner::OMPLPlannerThreadManage);
        AIBOX_LOGI("Creat a planner manage thread");
    }
}
void 
OMPLPlanner::OMPLPlannerThreadManage()
{
    while(1){
        _ompl_planner_list.remove_if([](const std::shared_ptr<OMPLPlanner>& value){
                return value->isRunComplete();
            });
        //sleep 10s for next check 
        std::this_thread::sleep_for(std::chrono::seconds(10));
    }
}